#include <stdio.h>
#include "udp_ctrl.h"
#include "lwip/err.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"
#include <lwip/netdb.h>
#include <string.h>
#include <sys/param.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_wifi.h"
#include "esp_log.h"
#include "esp_netif.h"
#include "protocol_common.h"
#include <string.h>

#define MC_TX_RX_BUF_SIZE   CONFIG_TX_RX_DATA_BUF_SIZE

#define MC_TX_IP    CONFIG_MC_IP
#define MC_TX_PORT  (CONFIG_MC_TX_PORT)

#define MC_RX_IP    CONFIG_MC_IP
#define MC_RX_PORT  (CONFIG_MC_RX_PORT)


int mc_tx_init(char* mc_ip)
{
    int err = 0;
    int tx_socket = socket(PF_INET, SOCK_DGRAM, 0);
    if(tx_socket<0)
    {
        printf("Err mc tx: create socket error:%d\n", errno);
        goto error;
    }

    int flag = 1;
    err = setsockopt(tx_socket, IPPROTO_IP, IP_MULTICAST_TTL, &flag, sizeof(flag));
    if(err<0)
    {
        printf("Err mc tx: setsockopt(ttl) error:%d\n", errno);
        goto error;
    }

    /*enable udp mc*/
    struct ip_mreq imreq = { 0 };
    imreq.imr_interface.s_addr = htonl(INADDR_ANY);
    err = inet_aton(mc_ip, &imreq.imr_multiaddr.s_addr);
    if(err != 1)
    {
        printf("Err mc tx: IP:%s is error", mc_ip);
        goto error;
    }
    err = setsockopt(tx_socket, IPPROTO_IP, IP_MULTICAST_IF, &imreq, sizeof(imreq));
    if(err<0)
    {
        printf("Err mc tx: setsockopt(if) error:%d\n", errno);
        goto error;
    }

    return tx_socket;
error:
    if(tx_socket>0)
    {
        close(tx_socket);
    }
    return -1;
}

int mc_rx_init(const char* mc_ip, uint16_t port)
{
    int rx_socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_IP);
    if(rx_socket < 0)
    {
        printf("Err mc rx: create socket error:%d\n", errno);
        goto error;
    }

    /*bind local port*/
    struct sockaddr_in saddr = {0};
    saddr.sin_family = AF_INET;
    saddr.sin_port = htons(port);
    saddr.sin_addr.s_addr = htonl(INADDR_ANY);
    int err = bind(rx_socket, (struct sockaddr*)&saddr, sizeof(struct sockaddr_in));
    if(err<0)
    {
        printf("Err mc rx: bind error:%d\n", errno);
        goto error;
    }

    struct ip_mreq imreq = { 0 };
    imreq.imr_interface.s_addr = htonl(INADDR_ANY);
    err = inet_aton(mc_ip, &imreq.imr_multiaddr.s_addr);
    if(err != 1)
    {
        printf("Err mc rx: IP:%s is error", mc_ip);
        goto error;
    }

    /*加入多播组*/
    err = setsockopt(rx_socket, IPPROTO_IP, IP_ADD_MEMBERSHIP, &imreq, sizeof(imreq));
    if(err<0)
    {
        printf("Err mc rx: setsockopt(add) error:%d\n", errno);
    }
    return rx_socket;

error:
    if(rx_socket>0)
    {
        close(rx_socket);
    }
    return -1;
}

static void ctr_rx_task(void* pv)
{
    /*receive buf*/
    char buf[MC_TX_RX_BUF_SIZE+1] = {0};
    ssize_t recv_len = 0;

    /*init*/
    int rx_socket = mc_rx_init(MC_RX_IP, MC_RX_PORT);
    if(rx_socket<0)
    {
        return;
    }
    printf("Start udp ctr_rx_task\n");

    while(1)
    {
        recv_len = recv(rx_socket, buf, MC_TX_RX_BUF_SIZE, 0);
        if(recv_len)
        {
            /*data fix*/
            buf[recv_len] = '\0';
            printf("receive data: %s\n", buf);
        }
    }
}

static void ctr_tx_task(void *pv)
{
    int tx_socket = mc_tx_init(MC_TX_IP);
    if(tx_socket<0)
    {
        return;
    }

    struct sockaddr_in saddr = {0};
    saddr.sin_family = AF_INET;
    saddr.sin_port = htons(MC_TX_PORT);
    int err = inet_aton(MC_TX_IP ,&saddr.sin_addr.s_addr);
    if(err <0)
    {
        printf("Err mc tx task: inet_aton err:%d\n", errno);
        return;
    }

    uint8_t data[MC_TX_RX_BUF_SIZE+1] = {0};
    uint8_t data_len = 0;
    printf("Start udp ctr_tx_task\n");
    while(1)
    {
        /*prepaire data*/
        strncpy((char*)data, "hello", MC_TX_RX_BUF_SIZE);
        data_len = strlen((char*)data);
        sendto(tx_socket, data, data_len, 0, (struct sockaddr*)&saddr, sizeof(saddr));
        vTaskDelay(1000/portTICK_PERIOD_MS);
    }
}


void udp_ctrl_init(void)
{
    xTaskCreate(ctr_rx_task, "udp_ctr_rx_task", 4096, NULL, 20, NULL);
    xTaskCreate(ctr_tx_task, "udp_ctr_tx_task", 4096, NULL, 21, NULL);
}
